/*
* Copyright (c) 2007 by Kirill Kolodyazhniy.
* See the file "license.terms" for information on usage and redistribution.
*/

#include "odephysicsystem.h"
#include "odespace.h"
#include "odesolid.h"

#include "atomic.h"

#ifdef _DEBUG
#ifdef _MSC_VER
   #define new new( _CLIENT_BLOCK, __FILE__, __LINE__)
#endif
#endif // _DEBUG

namespace Engine
{
	/***************************************************************************************************************/
	static const int MAX_CONTACTS = 8;
	static const REAL minERP = 0.01;
	static const REAL maxERP = 0.99;
	static const REAL globalCFM = 1e-5;
	static const REAL surfaceLayer = 0.01;
	static const REAL bounceThreshold = 1.0;
	static const REAL maxFriction = 1000.0;
	static const REAL autoDisableLinearThreshold = 0.2;
	static const REAL autoDisableAngularThreshold = 0.2;
	static const int autoDisableSteps = 60;
	static const REAL autoDisableTime = 0.4;
	/***************************************************************************************************************/
	OdePhysicSystem* OdePhysicSystemFactory::CreateInstance( const std::string& name)
	{
		return new OdePhysicSystem(name);
	}
	/***************************************************************************************************************/
	long OdePhysicSystem::m_odeInit = 0;
	/***************************************************************************************************************/
	OdePhysicSystem::OdePhysicSystem(const std::string& name)
		:PhysicSystem(name)
	{
		long prevInit = 0;
		prevInit = AtomicExchange(&m_odeInit,m_odeInit);
		if(prevInit == 0)
		{
			dInitODE();
			AtomicIncrement(&m_odeInit);
		}

		m_worldID = dWorldCreate();

		m_contactJointGroupID = dJointGroupCreate(0);

		dVector3 center = { 0, 0, 0 };
		dVector3 extents = { 10000,	10000,	10000 };
		m_rootSpaceID = dQuadTreeSpaceCreate(0, center, extents, 8);


		dWorldSetCFM(m_worldID, static_cast<dReal>(globalCFM));

		dWorldSetERP(m_worldID,static_cast<dReal>( 0.5 * (maxERP + minERP)));

		dWorldSetContactSurfaceLayer(m_worldID, static_cast<dReal>(surfaceLayer));

		dWorldSetAutoDisableFlag (m_worldID,1);
		dWorldSetAutoDisableLinearThreshold (m_worldID, static_cast<dReal>(autoDisableLinearThreshold));
		dWorldSetAutoDisableAngularThreshold (m_worldID, static_cast<dReal>(autoDisableAngularThreshold));
		dWorldSetAutoDisableSteps (m_worldID, autoDisableSteps);
		dWorldSetAutoDisableTime (m_worldID, static_cast<dReal>(autoDisableTime));
	}
	/***************************************************************************************************************/
	OdePhysicSystem::~OdePhysicSystem()
	{
		dSpaceDestroy(m_rootSpaceID);
		dWorldDestroy(m_worldID);
		dJointGroupDestroy(m_contactJointGroupID);

		long prevInit = 0;
		prevInit = AtomicExchange(&m_odeInit,m_odeInit);
		if(prevInit > 0)
		{
			AtomicDecrement(&m_odeInit);
			--prevInit;
		}
		if(prevInit <= 0)
			dCloseODE();
	}
	/***************************************************************************************************************/
	void OdePhysicSystem::SetGravity(REAL x, REAL y, REAL z)
	{
		dWorldSetGravity (m_worldID, static_cast<dReal>(x), static_cast<dReal>(y), static_cast<dReal>(z));
	}
	/***************************************************************************************************************/
	Space* OdePhysicSystem::CreateSpaceImpl(const std::string& name)
	{
		return new OdeSpace(m_rootSpaceID, name);
	}
	/***************************************************************************************************************/
	Solid* OdePhysicSystem::CreateSolidImpl(const std::string& name)
	{
		return new OdeSolid(m_worldID, m_rootSpaceID, name, m_rootNode->CreateChild(name));
	}
	/***************************************************************************************************************/
	void CollisionCallback(void *data, dGeomID o0, dGeomID o1)
	{
		if (dGeomIsSpace(o0) || dGeomIsSpace(o1))
		{
			// Colliding a space with either a geom or another space.
			dSpaceCollide2(o0, o1, data, &CollisionCallback);

			if (dGeomIsSpace(o0))
			{
				// Colliding all geoms internal to the space.
				dSpaceCollide(reinterpret_cast<dSpaceID>(o0), data, &CollisionCallback);
			}

			if (dGeomIsSpace(o1))
			{
				// Colliding all geoms internal to the space.
				dSpaceCollide(reinterpret_cast<dSpaceID>(o1), data, &CollisionCallback);
			}
		}
		else
		{
			OdePhysicSystem* sim = reinterpret_cast<OdePhysicSystem*>(data);
			// exit without doing anything if the two bodies are connected by a joint
			dBodyID b0 = dGeomGetBody(o0);
			dBodyID b1 = dGeomGetBody(o1);
			if (b0 && b1 && dAreConnectedExcluding (b0,b1,dJointTypeContact))
				return;

			GeomData* geomData0 = static_cast<GeomData*>(dGeomGetData(o0));
			GeomData* geomData1 = static_cast<GeomData*>(dGeomGetData(o1));

			ShapeData* shape0 = geomData0->GetShapeData();
			ShapeData* shape1 = geomData1->GetShapeData();

			dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
			for (int i=0; i < MAX_CONTACTS; ++i)
			{
				const PhysicMaterialPtr m0 = shape0->material;
				const PhysicMaterialPtr m1 = shape1->material;

				contact[i].surface.mode = dContactBounce | dContactSoftERP;

				contact[i].surface.bounce = static_cast<dReal>((m0->bounciness + m1->bounciness) * 0.5);

				contact[i].surface.bounce_vel = static_cast<dReal>(bounceThreshold);

				if (1.0 == m0->friction && 1.0 == m1->friction)
					contact[i].surface.mu = static_cast<dReal>(dInfinity);
				else
					contact[i].surface.mu = static_cast<dReal>(sqrt(m0->friction * m1->friction) * maxFriction);

				// Average the hardness of the two materials.
				REAL hardness = (m0->hardness + m1->hardness) * 0.5;

				// Convert hardness to ERP.  As hardness goes from	0.0 to 1.0, ERP goes from min to max.
				contact[i].surface.soft_erp = static_cast<dReal>(hardness * (maxERP - minERP) + minERP);
			}
			if (int numc = dCollide (o0,o1,MAX_CONTACTS,&contact[0].geom, sizeof(dContact)))
			{
				for (int i=0; i < numc; ++i)
				{
					dJointID c = dJointCreateContact (sim->m_worldID, sim->m_contactJointGroupID, contact + i);
					dJointAttach (c,b0,b1);
					Vector3<REAL> colPos(contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2]);
					Vector3<REAL> colNorm(contact[i].geom.normal[0], contact[i].geom.normal[1], contact[i].geom.normal[2]);

					if(contact[i].geom.depth > shape0->collisionDepth)
						(*shape0->CollisionSignal.GetPointer())(shape0->entity, colPos, colNorm, contact[i].geom.depth);

					if(contact[i].geom.depth > shape1->collisionDepth)
						(*shape1->CollisionSignal.GetPointer())(shape1->entity, colPos, colNorm, contact[i].geom.depth);
				}
			}
		}
	}
	/***************************************************************************************************************/
	void OdePhysicSystem::Simulate(REAL step)
	{
		// Do collision detection; add contacts to the contact joint group.
		dSpaceCollide(m_rootSpaceID, this, &CollisionCallback);

		// Take a simulation step.
		dWorldStep(m_worldID, static_cast<dReal>(step));

		// Remove all joints from the contact group.
		dJointGroupEmpty(m_contactJointGroupID);
	}
	/***************************************************************************************************************/
}
